// app
#include "robot_def.h"
#include "robot_cmd.h"
// module
#include "remote_control.h"
#include "message_center.h"
#include "general_def.h"
 #include "mpu6500.h"
#include "arm_solution.h"
// bsp
#include "bsp_dwt.h"

//static Openmv_Recv_s *openmv_data; // openmv数据,初始化时返回

static RC_ctrl_t *rc_data; // 遥控器数据,初始化时返回
static attitude_t *IMU_data; // IMU数据

static Publisher_t *manipulator_pub;
static Manipulator_Ctrl_Cmd_s Manipulator_cmd_recv; // 来自cmd发布的控制信息
static Subscriber_t *manipulator_sub;
static Manipulator_Upload_Data_s Manipulator_feedback_data; // 来自电机的反馈信息信息

static float left_right = 0;
static float up_down = 0;
static float forward_backward = 0;

static void RemoteScan();
static void CMDLimit(float *value ,float up_limit ,float dowm_limit);

/* 测试用 */
float cmd1[6] = {85.96968467,-51.44208418,80.72963858,0,0,0};
float out1[6] = {0};
uint32_t DWT_CNT;
float dt;
int range_flag;
float range[6];

void RobotCMDInit()
{
    IMU_data = INS_Init();
    rc_data = RemoteControlInit(&huart1); // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个

    manipulator_pub = PubRegister("manipulator_cmd", sizeof(Manipulator_Ctrl_Cmd_s));
    manipulator_sub = SubRegister("manipulator_feed", sizeof(Manipulator_Upload_Data_s));
}

static void RemoteScan()
{
    if(rc_data->rc.rocker_r_ > 0)       left_right += 0.1;
    else if(rc_data->rc.rocker_r_ < 0)  left_right -= 0.1;

    if(rc_data->rc.rocker_r1 > 0)       up_down += 0.1;
    else if(rc_data->rc.rocker_r1 < 0)  up_down -= 0.1;

    if(rc_data->rc.rocker_l1 > 0)       forward_backward += 0.1;
    else if(rc_data->rc.rocker_l1 < 0)  forward_backward -= 0.1;

}

/* 控制限幅 */
static void CMDLimit(float *value ,float up_limit ,float dowm_limit)
{   
    if( *value <= dowm_limit) *value = dowm_limit;
    if( *value >= up_limit)   *value = up_limit;
}

// 模式选择左键 模式内作用右键
void RobotCMDTask()
{   
    //待完善
    //1.逆解算结果滤波
    //2.避免出现突变的情况（由于多解造成的）
    //3.对无解的情况进行排除
    //4.给初始值，避免一开始就是无解


    if (switch_is_down(rc_data[TEMP].rc.switch_right)&&switch_is_down(rc_data[TEMP].rc.switch_left)) // 右侧开关状态[下],左侧开关状态[下]
    {
        //开始任务
        Manipulator_cmd_recv.mode = TASK_MODE;

        // 获取电机此时角度
        SubGetMessage(manipulator_sub, &Manipulator_feedback_data);

        RemoteScan();
        CMDLimit(&forward_backward,250, -80);
        CMDLimit(&left_right,      250,-250);
        CMDLimit(&up_down,         360, 70);

        range[0] = forward_backward;
        range[1] = left_right;
        range[2] = up_down;

        range_flag = RangeLimit(range,6);
        if(range_flag == 1)
        {
            Manipulator_cmd_recv.pos[0] = forward_backward ;
            Manipulator_cmd_recv.pos[1] = left_right ;
            Manipulator_cmd_recv.pos[2] = up_down ;
        }
    
        /* 计算逆解算时间 */
        dt = DWT_GetDeltaT((void *)&DWT_CNT);
        InverseK(Manipulator_cmd_recv.pos, out1);
        dt = DWT_GetDeltaT((void *)&DWT_CNT);

        /* 角度限幅 */
        //AngleLimit(out1,6);
    }
    else if (switch_is_mid(rc_data[TEMP].rc.switch_right)&&switch_is_down(rc_data[TEMP].rc.switch_left)) // 右侧开关状态[中],左侧开关状态[下]
    {
        Manipulator_cmd_recv.mode = TEST_MODE;
    }
    else if (switch_is_mid(rc_data[TEMP].rc.switch_left))
    {
        Manipulator_cmd_recv.mode = STAST_MODE;
    }
    else if (switch_is_up(rc_data[TEMP].rc.switch_left))
    {
        Manipulator_cmd_recv.mode = STOP_MODE;
    }

    PubPushMessage(manipulator_pub, (void *)&Manipulator_cmd_recv);
    
}
